#include <Types.h>
#include <Hal/SSE/xmmintrin.h>

namespace FastLib
{

	/* TODO: create or find SSE2 enabled functions for memset, strcpy, strcmp, memcpy */
	void *Memcpy(void *dest, const void *src, size_t n)
	{
		char *d = (char *) dest;
		const char *s = (const char *) src;

		while (n--)
			*d++ = *s++;

		return dest;
	}

	void *Memset(void *dest, int val, size_t n)
	{
		char *d = (char *) dest;
		while (n > 0)
			d[--n] = (char) val;

		return dest;
	}

	void *Strcpy(char *dest, const char *src)
	{
		while ((*dest++ = *src++) != 0)
			;

		return dest;
	}

	void *Strncpy(char *dest, const char *src, size_t n)
	{
		while ((*dest++ = *src++) != 0 && n--)
			;

		return dest;
	}

	int Strcmp(const char *s1, const char *s2)
	{
		while (*s1++ == *s2++ && *s1 && *s2)
			;

		if (*s1 == 0 && *s2 == 0)
			return 0;

		if (*s1 > *s2)
			return 1;
		else
			return -1;
	}

	int Strncmp(const char *s1, const char *s2, size_t n)
	{
		while (*s1++ == *s2++ && *s1 && *s2 && n--)
			;

		if (*s1 == 0 && *s2 == 0)
			return 0;

		if (*s1 > *s2)
			return 1;
		else
			return -1;

	}

	/* TODO: test. This is extremely fast*/
	size_t Strlen(const char *str)
	{
		register size_t len = 0;

		/* Align to 16-bytes. If 0 is found here... return */
		while ((((uintptr_t) str) & (sizeof (__m128i) - 1)) != 0)
		{
			if (*str++ == 0)
				return len;
			++len;
		}

		/* Wipe out xmm0 */
		__m128i xmm0 = _mm_setzero_si128();
		__m128i xmm1;
		int mask = 0;

		/* Find length using SSE2 instructions */
		for (;;)
		{
			xmm1 = _mm_load_si128((__m128i *) str);
			xmm1 = _mm_cmpeq_epi8(xmm1, xmm0);
			if ((mask = _mm_movemask_epi8(xmm1)) != 0)
			{
				len += __builtin_ctz(mask);
				break;
			}
			str += sizeof (__m128i);
			len += sizeof (__m128i);
		}
		return len;
	}
}
